#!/usr/bin/env python

import roslib; roslib.load_manifest('simple_grasper')

from time import sleep
import math
from copy import copy

from pywam.wam import WAM
from pywam.hand import Grasper
from simple_grasper import GraspTestTrajectoryGenerator
from simple_grasper.srv import *

import rospy

def handle_egt(req):
    params = req.params
    length = params.length
    print length
    if length==123456789:
        #this is a special mode where all we want to do is raise J1 by 30 degrees
        wam = WAM()
        joint_values = wam.get_joint_pos()
        new_joint_values = copy(list(joint_values))
        new_joint_values[1] -= 30*math.pi/180
        wam.go_to_sync(new_joint_values,vlimit=0.5)
        wam.go_to_sync(joint_values,vlimit=0.5)
        hand = Grasper()
        hand.set_hand_position((0,0,0,0))
        wam.go_to((0,0,0,0,0,0,0),vlimit=0.8)
        return ExecuteGraspTestResponse(True)
    if length==7357:
        fname = "/tmp/rotation.txt"
        wam.play_trajectory(fname,timestep=0.1)
        return ExecuteGraspTestResponse(True)
    traj = params.trajectory
    print "length: ",len(traj.waypoints)
    print "correct: ",str(length)
    fname = "/tmp/traj.txt"
    #trajGen = GraspTestTrajectoryGenerator(soln)
    #traj = trajGen.compute_hand_over_trajectory(8)
    #trajGen.generate_trajectory_file(fname,traj)
    f = file(fname,'w')
    [f.write('0, '+', '.join(str(v) for v in wp.joints)+'\n') for wp in traj.waypoints]
    f.close()
    wam = WAM()
    hand = Grasper()
    def hybrid_close(hand):
        hand.set_hand_position((11000,11000,11000,0))
        sleep(2.0)
    #hand.set_hand_velocities((30,30,30))

    wam.play_trajectory(fname,timestep=0.15)
    wam.wait()

    hybrid_close(hand)

    return ExecuteGraspTestResponse(True)
def execute_grasp_test_server():
    rospy.init_node('execute_grasp_test_server')
    s = rospy.Service('execute_grasp_test',ExecuteGraspTest,handle_egt)
    wam = WAM()
    hand = Grasper()
    wam.go_to_sync((0,0,0,0,0,0,0),vlimit=0.8)
    hand.set_hand_position((0,0,0,0))
    
    print "Ready to Execute Grasp Test"
    rospy.spin()

if __name__ == "__main__":
    print "Note that the wam must be on and not in gravity comp, or this code \
    will hang until it is."
    execute_grasp_test_server()
